import cv2
import numpy as np
from library import *

img = cv2.imread('test_1.jpg')
imgL = img[0:480, 0:640, :]
imgR = img[0:480, 640:1280, :]

json_file = "config.json"
camera_matrix1, dist_coeffs1, camera_matrix2, dist_coeffs2, R, T = read_camera_parameters(json_file)
rectified_img1, rectified_img2, Q = BinocularImageRectify(imgL, imgR, camera_matrix1, dist_coeffs1, camera_matrix2,
                                                          dist_coeffs2, R, T)

dis, dis_color = getDepthImage(rectified_img1, rectified_img2, window_size=3, min_disp=0, num=8, blockSize=30,
                               uniqueness_ratio=10, speckle_range=32,
                               speckle_window_size=100, disp12_max_diff=1)

# cv2.imshow('dis',dis_color)
# cv2.waitKey(0)
# cv2.destroyAllWindows()
b, f, cx, cy = getBinocularSystemParameter(Q)
dis = dis[250, 300] / 16
print(dis)
distance = getTargetObjectDistance(dis, f, -b)
print(b, f, cx, cy)
print(distance)
# matrix = getThreeDimensionalImage(img, b, f, cx, cy, dis)
# draw_point_cloud(matrix)
